import open3d as o3d
import numpy as np
import  cv2


def gen_axis_frame_tr(t):
    axis_target = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1, origin=[0, 0, 0])
    axis_target.transform(t)
    return axis_target


def geometry_show(geometry_list):
    geom = o3d.geometry.AxisAlignedBoundingBox(min_bound=(-0.3, -0.3, -0.1), max_bound=(0.3, 0.3, 1.1))
    axis_pcd = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.2, origin=[0, 0, 0])
    geometry_list.append(geom)
    geometry_list.append(axis_pcd)
    lookat = np.array([0.0, 0.0, 0.0])
    up = np.array([0.0, -1.0, 0.0])
    front = np.array([0.0, 0.0, -1.0])
    zoom = 0.3
    o3d.visualization.draw_geometries(geometry_list,
                                      window_name='Open3D',
                                      width=1920,
                                      height=1080,
                                      left=50,
                                      top=50,
                                      point_show_normal=False,
                                      mesh_show_wireframe=False,
                                      mesh_show_back_face=False,
                                      lookat=lookat, up=up, front=front, zoom=zoom)


def gen_point_cloud(color_image, depth_image):
    o3d_color_image = o3d.geometry.Image(color_image)
    o3d_depth_image = o3d.geometry.Image(depth_image)
    rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
        o3d_color_image, o3d_depth_image, depth_trunc=3.0, depth_scale=1000.0, convert_rgb_to_intensity=False)
    o3d_intrinsic = o3d.camera.PinholeCameraIntrinsic()
    # o3d_intrinsic.set_intrinsics(1280, 720, 907.37, 905.473, 661.817, 376.51)
    o3d_intrinsic.set_intrinsics(848, 480, 602.327, 601.403, 425.902, 228.963)
    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(
        rgbd_image, o3d_intrinsic)
    return pcd

